1. Field of the Invention
The present invention relates in general to the use of variable-gain filters, such as Kalman filters to estimate the state of a system, and in particular to improved measurement fault detection in such filters.
2. Description of the Related Art
The discrete time Kalman filter was introduced by Rudolf Kalman in a “A New Approach to Linear Filtering and Prediction Problems,” in the Transactions of the ASME, Series D: Journal of Basic Engineering, Vol. 82, pp. 35-45 in 1960. The Kalman filter (KF) produces the state estimate with the smallest mean-square error for a linear time-variant system with white, Gaussian state and measurement noise. The Kalman filter extended the concept of least-squares estimation to dynamic systems. The concept of least-squares estimation dates back to Gauss, at least. The Kalman filter has found innumerable applications in navigation, tracking, process control, parameter estimation, and other areas. It has been extended to include nonlinear state models, nonlinear measurement models, non-white state and measurement noise, and correlation between state and measurement noise.
A succinct description of the Kalman filter can be found in Applied Optimal Estimation by A. Gelb (ed.) from The MIT Press, 1974 pp. 1-179 which is herein incorporated by reference. A summary of the basic filter and some notes on the notation follow from Gelb. The notation to be used in the description is given in Table 1. A superscripted “T” after a matrix is the matrix transpose, and a superscripted “−1” after a matrix is it's inverse. Again following from Gelb, the discrete KF processing steps are given in Table 2. These steps advance the state estimate and it's associated error covariance matrix from one time to another.
TABLE 1Notation for Kalman Filter DescriptionXt1|t2State vector estimate for the system at time t1 usingmeasurements up to and including those from time t2.ΦState transition matrixQProcess Noise MatrixRMeasurement Noise Covariance MatrixPt1|t2State error covariance matrix at time t1 usingmeasurements up to and including time t2ZMeasurement vector, a set of vector measurements fora given time.HThe measurement model matrix. This is the partialderivative of the measurement vector with respectto the state vector.IThe identity matrix
TABLE 2Five Discrete Kalman Filtering Processing StepsStepStep NameStep Processing1State PropagationXt+Δt|t = ΦXt|t2Covariance PropagationPt+Δt|t = ΦPt|tΦT + Q3Gain CalculationK = Pt+Δt|tHT(HPt+Δt|tHT + R)−14State UpdateXt+Δt|t+Δt = Xt+Δt|t + K(Z − HXt+Δt|t)5Covariance UpdatePt+Δt|t+Δt = (I − KH)Pt+Δt|t
An implementation issue with the KF is the matrix inverse called for in step 3, the gain calculation, from table 2. Computing the inverse is often a substantial burden, particularly in a real-time system. A common practice, therefore, has been to divide up the processing of the vector measurement (steps 3, 4, and 5 from Table 2) into a sequence of scalar measurement processing. If the measurement vector Z, has one element, in other words when we have a scalar measurement, the measurement derivative with respect to the state, H, is a row vector. In this case the gain calculation, step 3 from Table 2 above reduces to a division rather than a matrix inversion.
  K  =            PH      T                      HPH        T            +      R      If a measurement vector, Z, contains multiple elements, it can be partitioned into a set of scalar measurements. In the case of a diagonal measurement noise covariance matrix R, partitioning into scalar measurements is no more complicated than picking out the diagonal term from the matrix R, and processing it with the corresponding row of H and the corresponding element of the vector Z. This is generally referred to as sequential measurement processing. We can expand our notation to include sequential measurement processing such that ZA, ZB, . . . are elements of the original vector measurement Z; HA, HB, . . . are the corresponding rows of H; RA, RB, . . . are the corresponding diagonal elements of the noise covariance matrix. The sequential measurement processing KF then follows these steps. The gain and residual calculations have been broken out for discussion of the Chi-square test in subsequent paragraphs. The state and covariance notation has been augmented for sequential processing, such that Xt+Δt|t,A is the state vector estimate for time t+Δt given measurements up to time t and measurement A. This state, updated for measurement A, is the input state for steps 10 and 13 of measurement B processing, and so on.
TABLE 3Discrete Sequential Kalman Filter Processing StepsRepeatedStepStepsStep NameStep Processing1State PropagationXt+Δt|t = ΦXt|t2Covariance PropagationPt+Δt|t = ΦPt|tΦT + QLoop for each3, 9, 15 ...Residual VarianceΓA = HAPHAT + RAmeasurement:4, 10, 16 ...ResidualδA = ZA − HAXt+Δt|t A, B, C ...5, 11, 17 ...Chi-Square test statistic            χ      A        2    =                    δ        A            2              Γ      A       6, 12, 18 ...Gain CalculationKA = Pt+Δt|tHAT/ΓA7, 13, 19 ...State UpdateXt+Δt|t, A = Xt+Δt|t +KAδA8, 14, 20 ...Covariance UpdatePt+Δt|t, A = (I − KH)Pt+Δt|tIn the case where R is not diagonal, in other words the measurement noises are correlated, techniques may be used which transform the R matrix to diagonal form for easy partitioning. The necessary transformations are defined by G. Bierman in Factorization Methods for Discrete Sequential Estimation, Academic Press 1977, pp. 47-55 which is herein incorporated by reference.
It is possible to prove that the order in which a set of sequential measurements is processed in a linear system using the steps of Table 3 is irrelevant to the final state estimate. A simple one-dimensional example of this fact is given in Table 4. In Table 4, the two measurements are taken to be measurements of the state vector directly, thus H is a two-by-two identity matrix. The example is for the sequential measurement processing, not the state propagation, so it corresponds to steps 3 through 14 inclusive of Table 3. The Chi-square test in the example of Table 4, will be described subsequently. There are two processing columns in Table 4, one for measurement order A,B, and the other for B,A. While the intermediate steps are different, the final state and covariance estimates are the same. Thus, in the prior art, the order in which measurements from the same time have been processed in a sequential filter has been a matter of convenience, the measurements are processed in whatever order they are gathered from the measuring device or stored in memory, or some other mechanism.
TABLE 4Single State KF Sequential Measurement Processing ExampleStateTruth: X = 0 Estimate: Xt|t−Δt = 1State VariancePt|t−Δt = 1 (state estimate correctly modeled)Measurement ModelHA = HB = H = 1RA = RB = R = 1MeasurementsZA = −1 (One sigma measurement, correctly modeled)ZB = 2 (Two sigma measurement, correctly modeled)Measurement ProcessingA,BB,AOrderFirst Measurement ResidualΓA = 1 + 1 = 2ΓB = 1 + 1 = 2VarianceFirst Measurement ResidualδA = ZA − Xt|t−Δt = −2δB = ZB − Xt|t−Δt = 1First Measurement Three(−2)2 < (32 · 2) ?(1)2 < (32 · 2) ?Sigma Chi-Square TestPassedPassedFirst Measurement GainKA = Pt|t−Δt/ΓA = 1/2KB = Pt|t−Δt/ΓB = 1/2First Measurement StateXt|t−Δt, A = Xt|t−Δt + KAδA = 0Xt|t−Δt, B = Xt|t−Δt + KBδB = 3/2UpdateFirst Measurement StatePt|t−Δt, A = (1 − KA)Pt|t−Δt = 1/2Pt|t−Δt, B = (1 − KB)Pt|t−Δt = 1/2Variance Update Second Measurement Residual Variance      Γ    B    =                    1        2            +      1        =          3      2            Γ    A    =                    1        2            +      1        =          3      2       Second MeasurementδB = ZB − Xt|t−Δt,|A = 2δA = ZA − Xt|t−Δt, B = −5/2Residual Second Measurement Three Sigma Chi-Square Test                                          (            2            )                    2                <                              (                                          3                2                            ·                              3                2                                      )                    ?                                Passed                                                (                          -                              5                2                                      )                    2                <                              (                                          3                2                            ·                              3                2                                      )                    ?                                Passed       Second Measurement GainKB = Pt|t−Δt, A/ΓB = 1/3KA = Pt|t−Δt, B/ΓA = 1/3Second Measurement StateXt|t = Xt|t−Δt, A + KBδB = 2/3Xt|t = Xt|t−Δt, B + KAδA = 2/3UpdateState Measurement StatePt|t = (1 − KB)Pt|t−Δt, A = 1/3Pt|t = (1 − KA)Pt|t−Δt, A = 1/3Variance Update
One of the problems apparent to Kalman filter practitioners is that measurements are often generated by physical mechanisms which can result in measurement faults. These faults, by definition, are components of the measurement values that are not included in the measurement model. So, for example, if the measurement is a pseudorange measurement to a GPS satellite, and the direct ray to the satellite has been blocked but a reflected ray is tracked, the resulting measurement has a fault, errors that fall outside the model used to process the measurement. This condition is shown in FIG. 2. Measurement faults have at least two adverse affects; they corrupt the resultant estimate of the state, and their effects are not considered in the update of the error covariance matrix. After processing a measurement with a fault, the state error covariance matrix may no longer be an accurate representation of the state error.
This problem led to the development of Chi-square measurement fault detection, sometimes referred to as Chi-square residual testing or innovations variance testing. The purpose is to reject measurements that we believe have been corrupted by a fault, and therefore fall outside of our filter model. The test is a statistical hypothesis test for a random variable. The application of a Chi-square test to the measurement residual dates back, at least, to Mehra and Peschon [5] in a 1971 paper titled “An Innovations Approach to Fault Detection and Diagnosis in Dynamic Systems” in Automatica, Vol. 7, pp. 637-640, 1971.
The chi-square residual test is a test on the hypothesis that the measurement we are about to process is actually a member of the measurement set we have defined. We define the residual as:δ=Z−HXt+Δt|t The hypothesis is tested by forming the ratio of the observed residual to it's expected value, thus, for a scalar measurement.
      χ    2    =            δ      2                      HPH        T            +      R      The test would be passed if the computed value of chi-squared is less than a threshold value. A typical threshold of nine, for example, would be a three sigma test on the measurement, meaning we would expect 99.7% of normally distributed measurements to pass the rest. So if the value χ2 computed above is greater than nine, we have detected a failure at the three sigma level. This test is well-known and in common practice.
Since the residual variance, HPHT+R, is also part of the scalar gain calculation, the chi-square test is often conducted in the sequential processing of the scalar measurements. Referring back to Table 3, the residual variance has been computed in step 3, and the Chi-square test statistic was formulated in step 5. Thus the test is computed in-line with the sequential measurement processing. The example in Table 4 shows the application of the test, the ratio is computed and checked before the gain computation and the state update are completed for each measurement. A flowchart of this process is given in FIG. 1.
The term Satellite Positioning System (SATPS) is used to define a satellite-based system transmitting ranging signals for use in navigation. Examples of SATPS include the Global Positioning System (GPS), the Russian GLONASS system, and the proposed European Galileo system. In all of these systems the receiver develops pseudorange measurements to the transmitting satellites by synchronizing a code replica with the transmitted satellite code. Doppler measurements, and integrated doppler measurements, may be made as well, with integrated doppler measurements commonly being referred to as delta-pseudorange measurements. Since the inception of the GPS, Kalman filtering has been a popular method for estimating the navigation state from the pseudorange and delta-pseudorange measurements. The paper “Aircraft Navigation with the United Operational Phase of the NAVSTAR Global Positioning System” by L. R. Kruczynski in Global Positioning System, Volume 1, pp. 154-165, 1980 from the Institute of Navigation, from his 1977 conference paper, describes a Kalman filter used to process GPS measurements. In satellite positioning system measurement processing there is some prior art on measurement fault detection methodology. Much of this art is concerned with ad hoc attempts to define a measurement edit or weighting strategy to reduce the effects of multipath. In U.S. Pat. No. 5,883,595 by Colley, for example, a mechanism for de-weighting suspect measurements is disclosed based on the size of their residuals and an undisclosed reliability factor. In U.S. Pat. No. 5,590,043 by McBurney some adaptation of filter parameters on the basis of multipath presence is disclosed, but with no mechanism apparent for detection of the multipath. The existence of these methods indicates the relative lack of success of unaltered Chi-square residual test at least in the field of satellite navigation, and calls out the need for improved measurement fault detection and editing.
It can be seen then, that there is a need in the art for improved measurement fault detection in state estimation. It can also be seen that there is a need in the art for improved multipath detection in satellite positioning system receivers.